#include "stm32f1xx.h"
#include "mpu6050.h"
#include "i2c.h"
#include "math.h"
//#include "task.h"

//宏定义
 #define WHO_AM_I_REG 0x75	//设备ID寄存器，用于确认传感器是否正确连接
 #define PWR_MGMT_1_REG 0x6B	//电源管理寄存器，用于控制传感器的唤醒和睡眠模式
 #define SMPLRT_DIV_REG 0x19	//采样率分频寄存器，用于设置数据输出率
 #define ACCEL_CONFIG_REG 0x1C	//加速度计配置寄存器，用于设置加速度计的量程
 #define ACCEL_XOUT_H_REG 0x3B	//加速度计X轴高位数据寄存器，从这里开始读取加速度计数据
 #define ACCEL_XOUT_L_REG 0x3C	//加速度计X轴低位数据寄存器
 #define ACCEL_YOUT_H_REG 0x3D	//加速度计Y轴高位数据寄存器
 #define ACCEL_YOUT_L_REG 0x3E	//加速度计Y轴低位数据寄存器
 #define ACCEL_ZOUT_H_REG 0x3F	//加速度计Z轴高位数据寄存器
 #define ACCEL_ZOUT_L_REG 0x40	//加速度计Z轴低位数据寄存器
 #define TEMP_OUT_H_REG 0x41	//温度数据高位寄存器，用于读取温度数据
 #define TEMP_OUT_L_REG 0x42	//温度数据低位寄存器
 #define GYRO_CONFIG_REG 0x1B	//陀螺仪配置寄存器，用于设置陀螺仪的量程
 #define GYRO_XOUT_H_REG 0x43	//陀螺仪X轴高位数据寄存器，从这里开始读取陀螺仪数据
 #define GYRO_XOUT_L_REG 0x44	//陀螺仪X轴低位数据寄存器
 #define GYRO_YOUT_H_REG 0x45	//陀螺仪Y轴高位数据寄存器
 #define GYRO_YOUT_L_REG 0x46	//陀螺仪Y轴低位数据寄存器
 #define GYRO_ZOUT_H_REG 0x47	//陀螺仪Z轴高位数据寄存器
 #define GYRO_ZOUT_L_REG 0x48	//陀螺仪Z轴低位数据寄存器
 #define MPU6050_ADDR 0xD0	//MPU6050的I2C地址，0xD0是MPU6050的默认地址（写操作地址）
 #define PI 3.1415927f  //圆周率常量
 #define filter 0.95238f    //滤波器宏定义

static float ax, ay, az; //加速度计数据 单位g
static float gx, gy, gz; //陀螺仪数据 单位°/s
static float temperature; //温度数据 单位°C
static float yaw, pitch, roll; //欧拉角数据 单位°

static void reg_Write(uint8_t reg, uint8_t value);
static uint8_t reg_Read(uint8_t reg);

/*初始化MPU6050*/
void MPU6050_Init(void)
{
    reg_Write(PWR_MGMT_1_REG, 0x80); //复位传感器
    HAL_Delay(100); //延时100ms等待复位完成
	reg_Write(PWR_MGMT_1_REG, 0x00); //唤醒传感器
    reg_Write(ACCEL_CONFIG_REG, 0x00); //设置加速度计量程±2g
    reg_Write(GYRO_CONFIG_REG, 0x18); //设置陀螺仪量程±2000度/秒
    //reg_Write(SMPLRT_DIV_REG, 0x07); //设置采样率为1kHz
}

/*MPU6050进程函数 每5ms调用一次*/
void MPU6050_Proc(void)
{
    //PERIODIC(5);
    static uint32_t nxt = 0;//下次程序调用时间
    if (HAL_GetTick() < nxt) return;

    MPU6050_Update();//更新MPU6050的值
    float dt = 0.005f; //每次调用间隔5ms

    /*陀螺仪解算欧拉角 会漂移*/
    float yaw_g = yaw + (gz * dt);//偏航角
    float pitch_g = pitch + (gx * dt);//俯仰角
    float roll_g = roll + (gy * dt); //翻滚角

    /*加速度计解算欧拉角 有噪声*/
//  float yaw_a = 0.0f//偏航角
    float pitch_a = atan2(ay,az) / PI * 180.0f; //俯仰角
    float roll_a = atan2(ax,az) / PI * 180.0f; //翻滚角

    /*互补滤波器融合陀螺仪和加速度计数据*/
    yaw = yaw_g; //偏航角
    pitch = filter * (pitch_g) + (1 - filter) * (pitch_a); //俯仰角
    roll = filter * (roll_g) + (1 - filter) * (roll_a); //翻滚角
	
	
    
    nxt += 5; //设置下次调用时间
    if (nxt > 1000) nxt = 0; //防止溢出

}

/*更新MPU6050的值*/
void MPU6050_Update(void)
{
    int16_t ax_raw = (int16_t)((reg_Read(ACCEL_XOUT_H_REG) << 8) + reg_Read(ACCEL_XOUT_L_REG));//ax原始数据
    int16_t ay_raw = (int16_t)((reg_Read(ACCEL_YOUT_H_REG) << 8) + reg_Read(ACCEL_YOUT_L_REG));//ay原始数据
    int16_t az_raw = (int16_t)((reg_Read(ACCEL_ZOUT_H_REG) << 8) + reg_Read(ACCEL_ZOUT_L_REG));//az原始数据
    int16_t gx_raw = (int16_t)((reg_Read(GYRO_XOUT_H_REG) << 8) + reg_Read(GYRO_XOUT_L_REG));//gx原始数据
    int16_t gy_raw = (int16_t)((reg_Read(GYRO_YOUT_H_REG) << 8) + reg_Read(GYRO_YOUT_L_REG));//gy原始数据
    int16_t gz_raw = (int16_t)((reg_Read(GYRO_ZOUT_H_REG) << 8) + reg_Read(GYRO_ZOUT_L_REG));//gz原始数据
    int16_t temp_raw = (int16_t)((reg_Read(TEMP_OUT_H_REG) << 8) + reg_Read(TEMP_OUT_L_REG));//温度原始数据
    /*将原始数据转换为实际的加速度、角速度值*/
    ax = ax_raw * 6.1035e-5f;
    ay = ay_raw * 6.1035e-5f;
    az = az_raw * 6.1035e-5f;
    gx = gx_raw * 6.1035e-2f;
    gy = gy_raw * 6.1035e-2f;
    gz = gz_raw * 6.1035e-2f;
    temperature = (float)temp_raw / 340.0f + 36.53f; //mpu6050温度转换公式
}
/*获取各数据*/
float MPU6050_GetAx(void)
{
    return ax;
}
float MPU6050_GetAy(void)
{
    return ay;
}
float MPU6050_GetAz(void)
{
    return az;
}
float MPU6050_GetGx(void)
{
    return gx;
}
float MPU6050_GetGy(void)
{
    return gy;
}
float MPU6050_GetGz(void)
{
    return gz;
}
float MPU6050_GetTemperature(void)
{
    return temperature;
}
float MPU6050_GetYaw(void)
{
    return yaw;
}
float MPU6050_GetPitch(void)
{
    return pitch;
}
float MPU6050_GetRoll(void)
{
    return roll;
}

/*
    向寄存器写值
    reg:寄存器地址
    value:要写入的值
*/
static void reg_Write(uint8_t reg, uint8_t value)
{
    HAL_I2C_Mem_Write(&hi2c2, MPU6050_ADDR, reg, 1, &value, 1, 100);
}

/*
    从寄存器读取值
    reg:寄存器地址
    return:寄存器的值
*/
static uint8_t reg_Read(uint8_t reg)
{
    uint8_t value;

    HAL_I2C_Mem_Read(&hi2c2, MPU6050_ADDR, reg, 1, &value, 1, 100);

    return value;
}
